#include "SkeletonScriptReader.h"
#include "SplineCatmullRom.h"
#include "DecoLogger.h"
#include "SineWarp.h"

const double rampupTime = 0.0;

double CalculateAmplitudeRampup(double currentTime, double totalRampupTime);

double PiecewiseScriptEntry::EvaluateValue(double time)
{	
	double ret = 0;
	assert (time >= 0 && time <= mParam1);

	if (mFuncName == "lin")
	{
		double alpha = time / mParam1;
		ret = alpha * mParam3 + (1 - alpha) * mParam2;
	}


	return ret;

}
double PiecewiseScriptEntry::EvaluateDerivative(double time)
{
	double ret = 0;
	assert (time >= 0 && time <= mParam1);

	if (mFuncName == "lin")
	{
		return (mParam3 - mParam2) / mParam1;
	}

	return ret;
}


//double ContinuousScriptEntry::EvaluateValue(double time)
//{	
//	double ret = 0;
//
//	double amplitute = mParam1;
//	double period = mParam2;
//	double phaseShift = mParam3;
//	double constant = mParam4;
//
//	double omega = 2 * PI / period;
//	if (mFuncName == "sin")
//	{
//		ret = amplitute * sin(omega * time + phaseShift) + constant;
//	}
//	else if (mFuncName == "cos")
//	{
//		ret = amplitute * cos(omega * time + phaseShift) + constant;
//	}
//
//
//	return ret;
//
//}
//double ContinuousScriptEntry::EvaluateDerivative(double time)
//{
//	double ret = 0;
//
//	double amplitute = mParam1;
//	double period = mParam2;
//	double phaseShift = mParam3;
//	double constant = mParam4;
//	
//	double omega = 2 * PI / period;
//	if (mFuncName == "sin")
//	{
//		ret = amplitute * cos(omega * time + phaseShift) * omega;
//	}
//	else if (mFuncName == "cos")
//	{
//		ret = -amplitute * sin(omega * time + phaseShift) * omega;
//	}
//
//	return ret;
//}


ContinuousScriptEntry::ContinuousScriptEntry()
{
}

double ContinuousScriptEntry::EvaluateValue(double time)
{
	if(mParam7 != -1 && mParam8 != -1)
	{
		mParam1 = (mParam8 - mParam7) * 0.5f;
		mParam4 = (mParam8 + mParam7) * 0.5f;
	}
	SineWarp warp(mParam1, mParam3, mParam2, mParam4, mParam5, mParam10, mParam9);

	if (mCtrlPts.size() == 4)
	{
		std::vector<double> oriCtrPts;
		oriCtrPts.push_back(0.2);
		oriCtrPts.push_back(0.3);
		oriCtrPts.push_back(0.7);
		oriCtrPts.push_back(0.8);
		warp.SetOriginalKnotsOnTimeAxis(oriCtrPts);
		warp.SetCurrentKnotsOnTimeAxis(mCtrlPts);
	}
//	return warp.EvaluateValueBeizier(time);
	return warp.EvaluateValue(time);
}
double ContinuousScriptEntry::EvaluateDerivative(double time)
{
	if(mParam7 != -1 && mParam8 != -1)
	{
		mParam1 = (mParam8 - mParam7) * 0.5f;
		mParam4 = (mParam8 + mParam7) * 0.5f;
	}
	SineWarp warp(mParam1, mParam3, mParam2, mParam4, mParam5, mParam10, mParam9);

	if (mCtrlPts.size() == 4)
	{
		std::vector<double> oriCtrPts;
		oriCtrPts.push_back(0.2);
		oriCtrPts.push_back(0.3);
		oriCtrPts.push_back(0.7);
		oriCtrPts.push_back(0.8);
		warp.SetOriginalKnotsOnTimeAxis(oriCtrPts);
		warp.SetCurrentKnotsOnTimeAxis(mCtrlPts);
	}

	return warp.EvaluateDerivative(time);
}

ContinuousScriptEntry::~ContinuousScriptEntry()
{

}
void ContinuousScriptEntry::SetWarpControlPts(const std::vector<double>& ctrPts)
{

	mCtrlPts = ctrPts;
}

void ContinuousScriptEntry::SetWarpControlPts(int ithCtrlPt, double value)
{
	if (mCtrlPts.size() == 0)
	{
		mCtrlPts.push_back(0.2);
		mCtrlPts.push_back(0.3);
		mCtrlPts.push_back(0.7);
		mCtrlPts.push_back(0.8);
	}
	mCtrlPts[ithCtrlPt] = value;
}

SplineScriptEntry::SplineScriptEntry() : mSpline(NULL)
{
	mParam1 = 1.0; 
	mParam2 = 1.0; 
	mParam3 = 0.0; 
	mParam4 = 0.0;
}

SplineScriptEntry::SplineScriptEntry(const SplineScriptEntry& rhs) : mSpline(NULL) //not allowed
{
	
}

const SplineScriptEntry& SplineScriptEntry::operator= (const SplineScriptEntry& rhs) //not allowed
{
	return *this;
}

void SplineScriptEntry::SetControlPts(double period, const std::vector<double>& ctrlPts)
{
	if (mSpline)
		delete mSpline;
	mParam2 = period;
	
	std::vector<Vec2d> ctrl;
	if (ctrlPts.size() <= 3 || period <= 0)
	{
		(*DecoLogger::GetSingleton()) << "Period should be larger than 0 and size of ctrPts should be larger than 3\n";
	}
	int ctrlSize = static_cast<int>(ctrlPts.size());
	double stepSize = period / ctrlSize;
	ctrl.push_back(Vec2d(-2 * stepSize, ctrlPts[ctrlSize - 2]));
	ctrl.push_back(Vec2d(-1 * stepSize, ctrlPts[ctrlSize - 1])); //to get a periodic spline
	for (int i = 0; i < ctrlSize; ++i)
	{
		ctrl.push_back(Vec2d(i * stepSize, ctrlPts[i]));
	}
	ctrl.push_back(Vec2d(period, ctrlPts[0]));
	ctrl.push_back(Vec2d(period + stepSize, ctrlPts[1])); //to get a periodic spline
	mSpline = new SplineCatmullRom(ctrl, false);
}

double SplineScriptEntry::EvaluateValue(double time)
{
	double amplitude = mParam1;
	double period = mParam2;
	double phase = mParam3;
	time += phase;
	while (time < 0)
		time += period;
	if (time >= period)
	{
		int nCycles = static_cast<int>(time / period);
		time -= nCycles * period;
	}
	return amplitude * mSpline->getValue(time);
}
double SplineScriptEntry::EvaluateDerivative(double time)
{
	double amplitude = mParam1;
	double period = mParam2;
	double phase = mParam3;
	time += phase;
	while (time < 0)
		time += period;
	if (time >= period)
	{
		int nCycles = static_cast<int>(time / period);
		time -= nCycles * period;
	}
	return amplitude * mSpline->getSlope(time);
}



SplineScriptEntry::~SplineScriptEntry()
{
	if (mSpline)
		delete mSpline;
}

SkeletonScriptReader::SkeletonScriptReader(Model* model, const string& filePath) : mModel(model), mFileName(filePath), mCurrentEntryIndex(0), mLastStageTime(0)
{
	FILE* pfile = fopen(filePath.c_str(), "r");
	char scriptType[256];
	memset(scriptType, 0, 256 * sizeof(char));

	fscanf(pfile, "%s\n", scriptType);
	if (!strcmp(scriptType, "Piecewise"))
	{
		mType = ST_Piecewise;

	}
	else if (!strcmp(scriptType, "Continuous"))
	{
		mType = ST_Continuous;
	}
	else if (!strcmp(scriptType, "Spline"))
	{
		mType = ST_Spline;
	}
	else
	{
		fclose(pfile);
		return;
	}
	while (!feof(pfile))
	{
		SkeletonScriptEntry* entry = NULL;
		if (mType == ST_Spline)
		{
			entry = new SplineScriptEntry();
			char dofName[256];
			int n;
			float value;
			float period;
			std::vector<double> ctrlPts;
			memset(dofName, 0, 256 * sizeof(char));
			fscanf(pfile, "%s %f %d ", dofName, &period, &n);
			for (int i = 0; i < n; ++i)
			{
				fscanf(pfile, "%f ", &value);
				ctrlPts.push_back(value);
			}
			fscanf(pfile, "\n");
			entry->mDofName = dofName;
			entry->mFuncName = "";

			SplineScriptEntry* splineEntry = static_cast<SplineScriptEntry*>(entry);
			splineEntry->SetControlPts(period, ctrlPts);
			mEntries.push_back(splineEntry);

		}
		else
		{
			if (mType == ST_Piecewise)
				entry = new PiecewiseScriptEntry();
			else if (mType == ST_Continuous)
				entry = new ContinuousScriptEntry();

			char dofName[256], funcName[256];
			float amplitute, period, phaseShift, constant, straight1, straight2, min, max, half;
			int mod;
			memset(dofName, 0, 256 * sizeof(char));
			memset(funcName, 0, 256 * sizeof(char));
			fscanf(pfile, "%s %s %f %f %f %f %f %f %f %f %d %f\n", dofName, funcName, &(amplitute), &(period), &(phaseShift), &(constant), &(straight1), &(straight2), &(min), &(max), &(mod), &(half));
			entry->mDofName = dofName;
			entry->mFuncName = funcName;
			entry->mParam1 = amplitute;
			entry->mParam2 = period;
			entry->mParam3 = phaseShift;
			entry->mParam4 = constant;
			entry->mParam5 = straight1;
			entry->mParam6 = straight2;
			entry->mParam7 = min;
			entry->mParam8 = max;
			entry->mParam9 = bool(mod);
			entry->mParam10 = half;
			mEntries.push_back(entry);
		}

	}
	fclose(pfile);
}
SkeletonScriptReader::~SkeletonScriptReader()
{
	for (ScriptEntryListT::iterator it = mEntries.begin(); it != mEntries.end(); ++it)
	{
		delete *it;
	}
	mEntries.clear();
}

bool SkeletonScriptReader::SetAmplitude(const std::string& name, double value)
{
	if (mType == ST_Continuous || mType == ST_Spline)
	{
		for (ScriptEntryListT::iterator it = mEntries.begin(); it != mEntries.end(); ++it)
		{
			if ((*it)->mDofName == name)
			{
				(*it)->mParam1 = value;
				return true;
			}
		}
	}

	return false;
}

bool SkeletonScriptReader::SetPeriod(const std::string& name, double value)
{
	if (mType == ST_Continuous || mType == ST_Spline)
	{
		for (ScriptEntryListT::iterator it = mEntries.begin(); it != mEntries.end(); ++it)
		{
			if ((*it)->mDofName == name)
			{
				(*it)->mParam2 = value;
				return true;
			}
		}
	}

	return false;
}

bool SkeletonScriptReader::SetControlPts(const std::string& name, double period, const std::vector<double>& ctrPts)
{
	if (mType == ST_Spline)
	{
		for (ScriptEntryListT::iterator it = mEntries.begin(); it != mEntries.end(); ++it)
		{
			if ((*it)->mDofName == name)
			{
				SplineScriptEntry* entry = static_cast<SplineScriptEntry*>(*it);
				entry->SetControlPts(period, ctrPts);
				return true;
			}
		}

	}
	return false;
}

bool SkeletonScriptReader::SetRangeMin(const std::string& name, double value)
{
	if (mType == ST_Continuous)
	{
		for (ScriptEntryListT::iterator it = mEntries.begin(); it != mEntries.end(); ++it)
		{
			if ((*it)->mDofName == name)
			{
				(*it)->mParam7 = value;
				return true;
			}
		}
	}

	return false;
}

bool SkeletonScriptReader::SetRangeMax(const std::string& name, double value)
{
	if (mType == ST_Continuous)
	{
		for (ScriptEntryListT::iterator it = mEntries.begin(); it != mEntries.end(); ++it)
		{
			if ((*it)->mDofName == name)
			{
				(*it)->mParam8 = value;
				return true;
			}
		}
	}

	return false;
}

bool SkeletonScriptReader::SetPhase(const std::string& name, double value)
{
	if (mType == ST_Continuous || mType == ST_Spline)
	{
		for (ScriptEntryListT::iterator it = mEntries.begin(); it != mEntries.end(); ++it)
		{
			if ((*it)->mDofName == name)
			{
				(*it)->mParam3 = value;
				return true;
			}
		}
	}

	return false;
}


bool SkeletonScriptReader::SetConstant(const std::string& name, double value)
{
	if (mType == ST_Continuous || mType == ST_Spline)
	{
		for (ScriptEntryListT::iterator it = mEntries.begin(); it != mEntries.end(); ++it)
		{
			if ((*it)->mDofName == name)
			{
				(*it)->mParam4 = value;
				return true;
			}
		}
	}

	return false;
}


bool SkeletonScriptReader::SetStaright1(const std::string& name, double value)
{
	if (mType == ST_Continuous || mType == ST_Spline)
	{
		for (ScriptEntryListT::iterator it = mEntries.begin(); it != mEntries.end(); ++it)
		{
			if ((*it)->mDofName == name)
			{
				(*it)->mParam5 = value;
				return true;
			}
		}
	}

	return false;
}

bool SkeletonScriptReader::SetStaright2(const std::string& name, double value)
{
	if (mType == ST_Continuous || mType == ST_Spline)
	{
		for (ScriptEntryListT::iterator it = mEntries.begin(); it != mEntries.end(); ++it)
		{
			if ((*it)->mDofName == name)
			{
				(*it)->mParam6 = value;
				return true;
			}
		}
	}

	return false;
}

bool SkeletonScriptReader::SetHalfRatio(const std::string& name, double value)
{
	if (mType == ST_Continuous || mType == ST_Spline)
	{
		for (ScriptEntryListT::iterator it = mEntries.begin(); it != mEntries.end(); ++it)
		{
			if ((*it)->mDofName == name)
			{
				(*it)->mParam10 = value;
				return true;
			}
		}
	}

	return false;
}

bool SkeletonScriptReader::SetSineWarpControlPts(const std::string& name, const std::vector<double>& ctrPts)
{
	if (mType == ST_Continuous)
	{
		for (ScriptEntryListT::iterator it = mEntries.begin(); it != mEntries.end(); ++it)
		{
			ContinuousScriptEntry* entry = static_cast<ContinuousScriptEntry*> (*it);
			if (entry->mDofName == name)
			{
				entry->SetWarpControlPts(ctrPts);
				return true;
			}
		}
	}
	return false;
}

bool SkeletonScriptReader::SetSineWarpControlPts(const std::string& name, int ithCtrlPt, double value)
{
	if (mType == ST_Continuous)
	{
		for (ScriptEntryListT::iterator it = mEntries.begin(); it != mEntries.end(); ++it)
		{
			ContinuousScriptEntry* entry = static_cast<ContinuousScriptEntry*> (*it);
			if (entry->mDofName == name)
			{
				entry->SetWarpControlPts(ithCtrlPt, value);
				return true;
			}
		}
	}
	return false;
}

void SkeletonScriptReader::GetCurrentDofsAndVels(double time, double* dof, double* vel)
{
	int numDofs = mModel->getNumDofs();
	double currentTime = time;
	double ratio = CalculateAmplitudeRampup(time, rampupTime);

	memset(dof, 0, numDofs * sizeof(double));
	memset(vel, 0, numDofs * sizeof(double));
	
	if (mType == ST_Piecewise)
	{
		double timeInThisStage = 0;
		SkeletonScriptEntry* entry = getCurrentEntryAndTime(time, timeInThisStage);
		for (int i = 0; i < numDofs; ++i)
		{
			Dof* modelDofs = mModel->getDof(i);
			if (strstr(modelDofs->getName(), entry->mDofName.c_str()))
			{
				double configuration = ratio * entry->EvaluateValue(timeInThisStage);
				double velocity = ratio * entry->EvaluateDerivative(timeInThisStage);
				dof[i] = configuration;
				vel[i] = velocity;
			}

		}

	}
	else if (mType == ST_Continuous || mType == ST_Spline)
	{
		for (ScriptEntryListT::iterator it = mEntries.begin(); it != mEntries.end(); ++it)
		{

			SkeletonScriptEntry* entry = *it;
			for (int i = 0; i < numDofs; ++i)
			{
				Dof* modelDofs = mModel->getDof(i);
				if (strstr(modelDofs->getName(), entry->mDofName.c_str()))
				{
					double configuration = ratio * entry->EvaluateValue(time);
					double velocity = ratio * entry->EvaluateDerivative(time);
					dof[i] = configuration;
					vel[i] = velocity;
					//if (mType == ST_Spline)
					{
						dof[i] += mModel->getDof(i)->getInitValue();
					}
				}

			}
		}
	}
}


void SkeletonScriptReader::UpdateModel(double time)
{
	int numDofs = mModel->getNumDofs();
	double currentTime = time;
	double ratio = CalculateAmplitudeRampup(time, rampupTime);
	
	if (mType == ST_Piecewise)
	{
		double timeInThisStage = 0;
		SkeletonScriptEntry* entry = getCurrentEntryAndTime(time, timeInThisStage);
		for (int i = 0; i < numDofs; ++i)
		{
			Dof* dof = mModel->getDof(i);

			if (strstr(dof->getName(), entry->mDofName.c_str()))
			{

				double configuration = ratio * entry->EvaluateValue(timeInThisStage);
				double velocity = ratio * entry->EvaluateDerivative(timeInThisStage);
				dof->setValue(configuration);
				dof->setVelocity(velocity);
			}
		}

	}
	else if (mType == ST_Continuous || mType == ST_Spline)
	{
		for (ScriptEntryListT::iterator it = mEntries.begin(); it != mEntries.end(); ++it)
		{
			SkeletonScriptEntry* entry = *it;
			for (int i = 0; i < numDofs; ++i)
			{
				Dof* dof = mModel->getDof(i);
				if (strstr(dof->getName(), entry->mDofName.c_str()))
				{
					double configuration = ratio * entry->EvaluateValue(time);
					double velocity = ratio * entry->EvaluateDerivative(time);
					dof->setValue(configuration);
					dof->setVelocity(velocity);
				}

			}
		}
	}
}

SkeletonScriptEntry* SkeletonScriptReader::getCurrentEntryAndTime(double time, double& timeInThisStage)
{
	SkeletonScriptEntry* entry = mEntries[mCurrentEntryIndex];

	timeInThisStage = time - mLastStageTime;
	if (timeInThisStage > entry->mParam1)
	{
		mLastStageTime += entry->mParam1;
		timeInThisStage = time - mLastStageTime;
		if (mCurrentEntryIndex == mEntries.size() - 1)
			mCurrentEntryIndex = 0;
		else
			mCurrentEntryIndex++;
		return mEntries[mCurrentEntryIndex];
	}
	else
	{
		return entry;
	}
}

double CalculateAmplitudeRampup(double currentTime, double totalRampupTime)
{
	if(totalRampupTime == 0.0)
		return 1.0;

	double ratio = 1.0;
	if (currentTime <= totalRampupTime)
		ratio = currentTime / totalRampupTime;
	return ratio;
}